﻿//---------------------------------------------------------------------------


#pragma hdrstop

#include "math.h"
#include "Robot.h"
#include "GeneralParameter.h"

//---------------------------------------------------------------------------
#pragma package(smart_init)
//---------------------------------------------------------------------------

Robot::Robot(TWinControl *Sender, int motor_num)
{
	this->MotorNum = motor_num;
	this->LegType = TRADITIONAL;
	this->Name = "NoName";
	this->ID2GB = new int[motor_num];
	this->SyncLR = false;

	this->Robotis = new Motor*[this->MotorNum];
	for(int i=0; i < this->MotorNum; i++)
	{
		this->Robotis[i] = new Motor(Sender, i);
		this->Robotis[i]->SetComponent(i, VERTICAL, -150, -150);
		this->Robotis[i]->Component->MBar->OnChange = MotorComponentBar_Change;
		this->Robotis[i]->Component->MEdit->OnKeyPress = MotorComponentEdit_KeyPress;
		this->Robotis[i]->Component->MButtonP->OnClick = MotorComponentButton_Click;
		this->Robotis[i]->Component->MButtonN->OnClick = MotorComponentButton_Click;
		this->Robotis[i]->Component->MEdit->OnKeyPress = MotorComponentEdit_KeyPress;
		this->Robotis[i]->Component->MCheck->OnClick = MotorComponentCheck_Click;
	}

	for(int i=0; i < this->MotorNum; i++)
	{
		ID2GB[i] = -1;
	}

	for(int i=0; i < TOTAL_JOINT_NUM; i++)
	{
		Joint2GB[i] = -1;
		Joint2ID[i] = -1;
	}

	this->SetMirrorTable();
}
//---------------------------------------------------------------------------

Robot::~Robot()
{
	delete [] this->Robotis;
	delete [] this->ID2GB;
}
//---------------------------------------------------------------------------

int Robot::GetMotorNum()
{
	return this->MotorNum;
}
//---------------------------------------------------------------------------

void Robot::SetMirrorTable()
{
	MirrorTable[HEAD_PITCH] = HEAD_PITCH;
	MirrorTable[HEAD_YAW] = HEAD_YAW;
	MirrorTable[HEAD_ROLL] = HEAD_ROLL;
	////////////////////////////////////////////////////////////
	MirrorTable[WAIST_YAW] = WAIST_YAW;
	MirrorTable[WAIST_PITCH] = WAIST_PITCH;
	MirrorTable[WAIST_ROLL] = WAIST_ROLL;
	////////////////////////////////////////////////////////////
	MirrorTable[RIGHT_SHOULDER_YAW] = LEFT_SHOULDER_YAW;
	MirrorTable[RIGHT_SHOULDER_ROLL] = LEFT_SHOULDER_ROLL;
	MirrorTable[RIGHT_SHOULDER_PITCH] = LEFT_SHOULDER_PITCH;

	MirrorTable[RIGHT_WRIST_ROLL] = LEFT_WRIST_ROLL;
	MirrorTable[RIGHT_WRIST_PITCH] = LEFT_WRIST_PITCH;
	MirrorTable[RIGHT_WRIST_YAW] = LEFT_WRIST_YAW;

	MirrorTable[RIGHT_ELBOW_YAW] = LEFT_ELBOW_YAW;
	MirrorTable[RIGHT_ELBOW_PITCH] = LEFT_ELBOW_PITCH;
	MirrorTable[RIGHT_ELBOW_ROLL] = LEFT_ELBOW_ROLL;

	MirrorTable[RIGHT_HIP_PITCH] = LEFT_HIP_PITCH;
	MirrorTable[RIGHT_HIP_ROLL] = LEFT_HIP_ROLL;
	MirrorTable[RIGHT_HIP_YAW] = LEFT_HIP_YAW;

	MirrorTable[RIGHT_KNEE_YAW] = LEFT_KNEE_YAW;
	MirrorTable[RIGHT_KNEE_PITCH] = LEFT_KNEE_PITCH;
	MirrorTable[RIGHT_KNEE_ROLL] = LEFT_KNEE_ROLL;

	MirrorTable[RIGHT_ANKLE_YAW] = LEFT_ANKLE_YAW;
	MirrorTable[RIGHT_ANKLE_ROLL] = LEFT_ANKLE_ROLL;
	MirrorTable[RIGHT_ANKLE_PITCH] = LEFT_ANKLE_PITCH;
	////////////////////////////////////////////////////////////
	MirrorTable[LEFT_SHOULDER_YAW] = RIGHT_SHOULDER_YAW;
	MirrorTable[LEFT_SHOULDER_ROLL] = RIGHT_SHOULDER_ROLL;
	MirrorTable[LEFT_SHOULDER_PITCH] = RIGHT_SHOULDER_PITCH;

	MirrorTable[LEFT_WRIST_ROLL] = RIGHT_WRIST_ROLL;
	MirrorTable[LEFT_WRIST_PITCH] = RIGHT_WRIST_PITCH;
	MirrorTable[LEFT_WRIST_YAW] = RIGHT_WRIST_YAW;

	MirrorTable[LEFT_ELBOW_YAW] = RIGHT_ELBOW_YAW;
	MirrorTable[LEFT_ELBOW_PITCH] = RIGHT_ELBOW_PITCH;
	MirrorTable[LEFT_ELBOW_ROLL] = RIGHT_ELBOW_ROLL;

	MirrorTable[LEFT_HIP_PITCH] = RIGHT_HIP_PITCH;
	MirrorTable[LEFT_HIP_ROLL] = RIGHT_HIP_ROLL;
	MirrorTable[LEFT_HIP_YAW] = RIGHT_HIP_YAW;

	MirrorTable[LEFT_KNEE_YAW] = RIGHT_KNEE_YAW;
	MirrorTable[LEFT_KNEE_PITCH] = RIGHT_KNEE_PITCH;
	MirrorTable[LEFT_KNEE_ROLL] = RIGHT_KNEE_ROLL;

	MirrorTable[LEFT_ANKLE_YAW] = RIGHT_ANKLE_YAW;
	MirrorTable[LEFT_ANKLE_ROLL] = RIGHT_ANKLE_ROLL;
	MirrorTable[LEFT_ANKLE_PITCH] = RIGHT_ANKLE_PITCH;
}
//---------------------------------------------------------------------------

void Robot::SetID2GB()
{
	for(int i=0; i < this->MotorNum; i++)
	{
    ID2GB[i] = -1;
  }
	for (int GBid = 0; GBid < this->MotorNum; GBid++) {
		if (Robotis[GBid]->ID >= 0 &&
				Robotis[GBid]->ID < this->MotorNum)
		{
			ID2GB[this->Robotis[GBid]->ID] = GBid;
		}
	}
}
//---------------------------------------------------------------------------

void Robot::SetJoint2GB()
{
	// If any of Motor->MotorComponent->JointNo is changed
	// Recalculate Joint2GB.
	for(int i=0; i < TOTAL_JOINT_NUM; i++)
	{
		Joint2GB[i] = -1;
	}
	for (int GBid = 0; GBid < this->MotorNum; GBid++) {
		if (Robotis[GBid]->JointNo >= 0 &&
				Robotis[GBid]->JointNo < TOTAL_JOINT_NUM)
		{
			Joint2GB[Robotis[GBid]->JointNo] = GBid;
		}
	}
}
//---------------------------------------------------------------------------

void Robot::SetJoint2ID()
{
	for(int i=0; i < TOTAL_JOINT_NUM; i++)
	{
		Joint2ID[i] = -1;
	}
	for (int GBid = 0; GBid < this->MotorNum; GBid++) {
		if (Robotis[GBid]->JointNo >= 0 &&
				Robotis[GBid]->JointNo < TOTAL_JOINT_NUM)
		{
			Joint2ID[Robotis[GBid]->JointNo] = Robotis[GBid]->ID;
    }
	}
}
//---------------------------------------------------------------------------

BodyJointStatus Robot::GetMirrorJoint(BodyJointStatus joint)
{
	if((joint >= 0) && (joint < TOTAL_JOINT_NUM))
	{
		return MirrorTable[joint];
	}
	else
	{
    return -1;
  }
}
//---------------------------------------------------------------------------

int Robot::GetID2GB(int id)
{
	if((id >= 0) && (id < this->MotorNum))
	{
		return ID2GB[id];
	}
	else
	{
    return -1;
  }
}
//---------------------------------------------------------------------------

int Robot::GetJoint2GB(BodyJointStatus joint)
{
	if((joint >= 0) && (joint < TOTAL_JOINT_NUM))
	{
		return Joint2GB[joint];
	}
	else
	{
    return -1;
  }
}
//---------------------------------------------------------------------------

int Robot::GetJoint2ID(BodyJointStatus joint)
{
	if((joint >= 0) && (joint < TOTAL_JOINT_NUM))
	{
		return Joint2ID[joint];
	}
	else
	{
    return -1;
  }
}
//---------------------------------------------------------------------------

bool Robot::SetGBColor(int GBid)
{
	BodyJointStatus joint = this->Robotis[GBid]->JointNo;

	for(int axis=0; axis < ROTATE_AXIS_NUM; axis++)
	{
		if(joint == HeadTable[axis] || joint == WaistTable[axis])
		{
			this->Robotis[GBid]->Component->MShape->Brush->Color = clGrayText;
			return true;
		}
	}

	for(int axis=0; axis < ROTATE_AXIS_NUM; axis++)
	{
		for(int part=0; part < HAND_JOINT_NUM; part++)
		{
			if (joint == RightHandTable[part][axis] ||
					joint == LeftHandTable[part][axis])
			{
				this->Robotis[GBid]->Component->MShape->Brush->Color = clMoneyGreen;
				return true;
			}
		}
    for(int part=0; part < LEG_JOINT_NUM; part++)
		{
			if (joint == RightLegTable[part][axis] ||
					joint == LeftLegTable[part][axis])
			{
				this->Robotis[GBid]->Component->MShape->Brush->Color = clTeal;
				return true;
			}
		}
	}

	return false;
}
//---------------------------------------------------------------------------

void Robot::SetAllGBColor()
{
	for(int i=0; i < this->MotorNum; i++)
	{
    this->SetGBColor(i);
  }
}
//---------------------------------------------------------------------------

void __fastcall Robot::MotorComponentBar_Change(TObject *Sender)
{
	int GBid = -1;

	for(int i=0; i < this->MotorNum; i++)
	{
		if(Sender == this->Robotis[i]->Component->MBar)
		{
			GBid = i;
			break;
    }
	}

	if((GBid >= 0) && (GBid < this->MotorNum))
	{
		this->Robotis[GBid]->ComponentBar_Change(Sender);

		if(this->SyncLR)
		{
			BodyJointStatus symmetry_joint;
			int symmetry_GBid;
			int p_max = this->Robotis[GBid]->GetPositionMax();
			int position;

			int polar, s_polar, temp, s_temp;
			int bar, sbar;

			symmetry_joint = this->GetMirrorJoint(this->Robotis[GBid]->JointNo);

			if(symmetry_joint != this->Robotis[GBid]->JointNo)
			{
				symmetry_GBid = this->GetJoint2GB(symmetry_joint);

				if((symmetry_GBid > 0) && (symmetry_GBid < this->MotorNum))
				{
					if(this->Robotis[GBid]->Component->GetDirection() == VERTICAL)
					{
						position = this->Robotis[GBid]->Component->MBar->Position;
					}
					else
					{
						position = this->Robotis[GBid]->GetPositionMax() - this->Robotis[GBid]->Component->MBar->Position;
          }

					this->Robotis[symmetry_GBid]->Component->MBar->Position = position;
				}
			}
		}
	}
}
//---------------------------------------------------------------------------

void __fastcall Robot::MotorComponentButton_Click(TObject *Sender)
{
  int GBid = -1;

	for(int i=0; i < this->MotorNum; i++)
	{
		if(Sender == this->Robotis[i]->Component->MButtonP ||
			 Sender == this->Robotis[i]->Component->MButtonN)
		{
			GBid = i;
			break;
    }
	}

	if((GBid >= 0) && (GBid < this->MotorNum))
	{
		this->Robotis[GBid]->ComponentButton_Click(Sender);

		if(this->SyncLR)
		{
			BodyJointStatus symmetry_joint;
			int symmetry_GBid;
			int position;

			symmetry_joint = this->GetMirrorJoint(this->Robotis[GBid]->JointNo);

			if(symmetry_joint != this->Robotis[GBid]->JointNo)
			{
				symmetry_GBid = this->GetJoint2GB(symmetry_joint);

				if((symmetry_GBid > 0) && (symmetry_GBid < this->MotorNum))
				{
					if(this->Robotis[GBid]->Component->GetDirection() == VERTICAL)
					{
						position = this->Robotis[GBid]->Component->MBar->Position;
					}
					else
					{
						position = this->Robotis[GBid]->GetPositionMax() - this->Robotis[GBid]->Component->MBar->Position;
          }

					this->Robotis[symmetry_GBid]->Component->MBar->Position = position;
				}
			}
		}
	}
}
//---------------------------------------------------------------------------

void __fastcall Robot::MotorComponentEdit_KeyPress(TObject *Sender, wchar_t &Key)
{
	if(Key == 13)
	{
		int GBid = -1;

		for(int i=0; i < this->MotorNum; i++)
		{
			if(Sender == this->Robotis[i]->Component->MEdit)
			{
				GBid = i;
				break;
			}
		}

		if((GBid >= 0) && (GBid < this->MotorNum))
		{
			this->Robotis[GBid]->ComponentEdit_KeyPress(Sender, Key);

			if(this->SyncLR)
			{
				BodyJointStatus symmetry_joint;
				int symmetry_GBid;
				int position;

				symmetry_joint = this->GetMirrorJoint(this->Robotis[GBid]->JointNo);

				if(symmetry_joint != this->Robotis[GBid]->JointNo)
				{
					symmetry_GBid = this->GetJoint2GB(symmetry_joint);

					if((symmetry_GBid > 0) && (symmetry_GBid < this->MotorNum))
					{
						if(this->Robotis[GBid]->Component->GetDirection() == VERTICAL)
						{
							position = this->Robotis[GBid]->Component->MBar->Position;
						}
						else
						{
							position = this->Robotis[GBid]->GetPositionMax() - this->Robotis[GBid]->Component->MBar->Position;
						}

						this->Robotis[symmetry_GBid]->Component->MBar->Position = position;
					}
				}
			}
		}
	}
}
//---------------------------------------------------------------------------

void __fastcall Robot::MotorComponentCheck_Click(TObject *Sender)
{
  int GBid = -1;

	for(int i=0; i < this->MotorNum; i++)
	{
		if(Sender == this->Robotis[i]->Component->MCheck)
		{
			GBid = i;
			break;
    }
	}

	if((GBid >= 0) && (GBid < this->MotorNum))
	{
		this->Robotis[GBid]->ComponentCheck_Click(Sender);
	}
}
//---------------------------------------------------------------------------

void Robot::LeftLeg_KA_Up()			//  ≥↑
{
	int GBid;

	GBid = this->GetJoint2GB(LEFT_KNEE_YAW);
	if((GBid >= 0) && (GBid < this->MotorNum))
	{
		this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonP);
	}

	if(this->LegType == TRADITIONAL)
	{
		GBid = this->GetJoint2GB(LEFT_ANKLE_YAW);
		if((GBid >= 0) && (GBid < this->MotorNum))
		{
			this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonN);
		}
	}
}
//---------------------------------------------------------------------------

void Robot::LeftLeg_KA_Down()		//  ≥↓
{
  int GBid;

	GBid = this->GetJoint2GB(LEFT_KNEE_YAW);
	if((GBid >= 0) && (GBid < this->MotorNum))
	{
		this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonN);
	}

	if(this->LegType == TRADITIONAL)
	{
		GBid = this->GetJoint2GB(LEFT_ANKLE_YAW);
		if((GBid >= 0) && (GBid < this->MotorNum))
		{
			this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonP);
		}
  }
}
//---------------------------------------------------------------------------

void Robot::RightLeg_KA_Up() 		//  ≤↑
{
  int GBid;

	GBid = this->GetJoint2GB(RIGHT_KNEE_YAW);
	if((GBid >= 0) && (GBid < this->MotorNum))
	{
		this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonP);
	}

	if(this->LegType == TRADITIONAL)
	{
		GBid = this->GetJoint2GB(RIGHT_ANKLE_YAW);
		if((GBid >= 0) && (GBid < this->MotorNum))
		{
			this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonN);
		}
  }
}
//---------------------------------------------------------------------------

void Robot::RightLeg_KA_Down()	//	≤↓
{
  int GBid;

	GBid = this->GetJoint2GB(RIGHT_KNEE_YAW);
	if((GBid >= 0) && (GBid < this->MotorNum))
	{
		this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonN);
	}

	if(this->LegType == TRADITIONAL)
	{
		GBid = this->GetJoint2GB(RIGHT_ANKLE_YAW);
		if((GBid >= 0) && (GBid < this->MotorNum))
		{
			this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonP);
		}
  }
}
//---------------------------------------------------------------------------

void Robot::BothLeg_KA_Up()			//  ≤↑≥
{
	this->LeftLeg_KA_Up();
	this->RightLeg_KA_Up();
}
//---------------------------------------------------------------------------

void Robot::BothLeg_KA_Down()		//  ≤↓≥
{
	this->LeftLeg_KA_Down();
	this->RightLeg_KA_Down();
}
//---------------------------------------------------------------------------

void Robot::LeftLeg_HK_Up()
{
	int GBid;

	GBid = this->GetJoint2GB(LEFT_HIP_YAW);
	if((GBid >= 0) && (GBid < this->MotorNum))
	{
		this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonN);
	}

	if(this->LegType == TRADITIONAL)
	{
		GBid = this->GetJoint2GB(LEFT_KNEE_YAW);
		if((GBid >= 0) && (GBid < this->MotorNum))
		{
			this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonP);
		}
	}
}
//---------------------------------------------------------------------------

void Robot::LeftLeg_HK_Down()
{
  int GBid;

	GBid = this->GetJoint2GB(LEFT_HIP_YAW);
	if((GBid >= 0) && (GBid < this->MotorNum))
	{
		this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonP);
	}

	if(this->LegType == TRADITIONAL)
	{
		GBid = this->GetJoint2GB(LEFT_KNEE_YAW);
		if((GBid >= 0) && (GBid < this->MotorNum))
		{
			this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonN);
		}
	}
}
//---------------------------------------------------------------------------

void Robot::RightLeg_HK_Up()
{
  int GBid;

	GBid = this->GetJoint2GB(RIGHT_HIP_YAW);
	if((GBid >= 0) && (GBid < this->MotorNum))
	{
		this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonN);
	}

	if(this->LegType == TRADITIONAL)
	{
		GBid = this->GetJoint2GB(RIGHT_KNEE_YAW);
		if((GBid >= 0) && (GBid < this->MotorNum))
		{
			this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonP);
		}
	}
}
//---------------------------------------------------------------------------

void Robot::RightLeg_HK_Down()
{
	int GBid;

	GBid = this->GetJoint2GB(RIGHT_HIP_YAW);
	if((GBid >= 0) && (GBid < this->MotorNum))
	{
		this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonP);
	}

	if(this->LegType == TRADITIONAL)
	{
		GBid = this->GetJoint2GB(RIGHT_KNEE_YAW);
		if((GBid >= 0) && (GBid < this->MotorNum))
		{
			this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonN);
		}
	}
}
//---------------------------------------------------------------------------

void Robot::BothLeg_HK_Up()
{
	this->LeftLeg_HK_Up();
	this->RightLeg_HK_Up();
}
//---------------------------------------------------------------------------

void Robot::BothLeg_HK_Down()
{
	this->LeftLeg_HK_Down();
	this->RightLeg_HK_Down();
}
//---------------------------------------------------------------------------

void Robot::LeftLeg_MoveOutward()
{
	int GBid;

	GBid = this->GetJoint2GB(LEFT_HIP_ROLL);
	if((GBid >= 0) && (GBid < this->MotorNum))
	{
		this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonP);
	}

	GBid = this->GetJoint2GB(LEFT_ANKLE_ROLL);
	if((GBid >= 0) && (GBid < this->MotorNum))
	{
		this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonN);
	}
}
//---------------------------------------------------------------------------

void Robot::LeftLeg_MoveInward()
{
  int GBid;

	GBid = this->GetJoint2GB(LEFT_HIP_ROLL);
	if((GBid >= 0) && (GBid < this->MotorNum))
	{
		this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonN);
	}

	GBid = this->GetJoint2GB(LEFT_ANKLE_ROLL);
	if((GBid >= 0) && (GBid < this->MotorNum))
	{
		this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonP);
	}
}
//---------------------------------------------------------------------------

void Robot::RightLeg_MoveOutward()
{
	int GBid;

	GBid = this->GetJoint2GB(RIGHT_HIP_ROLL);
	if((GBid >= 0) && (GBid < this->MotorNum))
	{
		this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonN);
	}

	GBid = this->GetJoint2GB(RIGHT_ANKLE_ROLL);
	if((GBid >= 0) && (GBid < this->MotorNum))
	{
		this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonP);
	}
}
//---------------------------------------------------------------------------

void Robot::RightLeg_MoveInward()
{
	int GBid;

	GBid = this->GetJoint2GB(RIGHT_HIP_ROLL);
	if((GBid >= 0) && (GBid < this->MotorNum))
	{
		this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonP);
	}

	GBid = this->GetJoint2GB(RIGHT_ANKLE_ROLL);
	if((GBid >= 0) && (GBid < this->MotorNum))
	{
		this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonN);
	}
}
//---------------------------------------------------------------------------

void Robot::BothLeg_LeanLeft()
{
	this->LeftLeg_MoveInward();
	this->RightLeg_MoveOutward();
}
//---------------------------------------------------------------------------

void Robot::BothLeg_LeanRight()
{
	this->LeftLeg_MoveOutward();
	this->RightLeg_MoveInward();
}
//---------------------------------------------------------------------------

void Robot::LeftLeg_Parallel_Up()
{
	this->LeftLeg_KA_Up();
	this->LeftLeg_HK_Up();
}
//---------------------------------------------------------------------------

void Robot::LeftLeg_Parallel_Down()
{
	this->LeftLeg_KA_Down();
	this->LeftLeg_HK_Down();
}
//---------------------------------------------------------------------------

void Robot::RightLeg_Parallel_Up()
{
	this->RightLeg_KA_Up();
	this->RightLeg_HK_Up();
}
//---------------------------------------------------------------------------

void Robot::RightLeg_Parallel_Down()
{
	this->RightLeg_KA_Down();
	this->RightLeg_HK_Down();
}
//---------------------------------------------------------------------------

void Robot::BothLeg_Parallel_Up()
{
	this->BothLeg_KA_Up();
	this->BothLeg_HK_Up();
}
//---------------------------------------------------------------------------

void Robot::BothLeg_Parallel_Down()
{
	this->BothLeg_KA_Down();
	this->BothLeg_HK_Down();
}
//---------------------------------------------------------------------------

void Robot::LeftLeg_Parallel_Forward()
{
	int GBid;

  GBid = this->GetJoint2GB(LEFT_HIP_YAW);
	if((GBid >= 0) && (GBid < this->MotorNum))
	{
		this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonN);
	}

	if(this->LegType == TRADITIONAL)
	{
		GBid = this->GetJoint2GB(LEFT_ANKLE_YAW);
		if((GBid >= 0) && (GBid < this->MotorNum))
		{
			this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonP);
		}
	}
	else if(this->LegType == PARALLEL)
	{
		GBid = this->GetJoint2GB(LEFT_KNEE_YAW);
		if((GBid >= 0) && (GBid < this->MotorNum))
		{
			this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonN);
		}
	}
}
//---------------------------------------------------------------------------

void Robot::LeftLeg_Parallel_Backward()
{
  int GBid;

  GBid = this->GetJoint2GB(LEFT_HIP_YAW);
	if((GBid >= 0) && (GBid < this->MotorNum))
	{
		this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonP);
	}

	if(this->LegType == TRADITIONAL)
	{
		GBid = this->GetJoint2GB(LEFT_ANKLE_YAW);
		if((GBid >= 0) && (GBid < this->MotorNum))
		{
			this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonN);
		}
	}
	else if(this->LegType == PARALLEL)
	{
		GBid = this->GetJoint2GB(LEFT_KNEE_YAW);
		if((GBid >= 0) && (GBid < this->MotorNum))
		{
			this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonP);
		}
	}
}
//---------------------------------------------------------------------------

void Robot::RightLeg_Parallel_Forward()
{
	int GBid;

  GBid = this->GetJoint2GB(RIGHT_HIP_YAW);
	if((GBid >= 0) && (GBid < this->MotorNum))
	{
		this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonN);
	}

	if(this->LegType == TRADITIONAL)
	{
		GBid = this->GetJoint2GB(RIGHT_ANKLE_YAW);
		if((GBid >= 0) && (GBid < this->MotorNum))
		{
			this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonP);
		}
	}
	else if(this->LegType == PARALLEL)
	{
		GBid = this->GetJoint2GB(RIGHT_KNEE_YAW);
		if((GBid >= 0) && (GBid < this->MotorNum))
		{
			this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonN);
		}
	}
}
//---------------------------------------------------------------------------

void Robot::RightLeg_Parallel_Backward()
{
	int GBid;

  GBid = this->GetJoint2GB(RIGHT_HIP_YAW);
	if((GBid >= 0) && (GBid < this->MotorNum))
	{
		this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonP);
	}

	if(this->LegType == TRADITIONAL)
	{
		GBid = this->GetJoint2GB(RIGHT_ANKLE_YAW);
		if((GBid >= 0) && (GBid < this->MotorNum))
		{
			this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonN);
		}
	}
	else if(this->LegType == PARALLEL)
	{
		GBid = this->GetJoint2GB(RIGHT_KNEE_YAW);
		if((GBid >= 0) && (GBid < this->MotorNum))
		{
			this->MotorComponentButton_Click(this->Robotis[GBid]->Component->MButtonP);
		}
	}
}
//---------------------------------------------------------------------------

bool Robot::InverseKinematics(float px, float py, float pz, float hip_yaw_angle, int motor_index[5], int* joint_position)
{
	float a3 = THIGH_Z;
	float a5 = THIGH_Z;
	float a4 = KNEE_Z;

	float foot_jointAng[5];
	double theta1;
	double theta2;
	double theta3;
	double theta5;
	double theta6;

	for(int i=0; i<5; i++)
	{
		 if((motor_index[i] < 0) || (motor_index[i] >= this->MotorNum))
		 {
       return false;
     }
	}

	theta1 = hip_yaw_angle * M_PI / 180;

	double B = px*sin(theta1) - py*cos(theta1);
	theta2 = atan2((px-B*sin(theta1)),(pz*cos(theta1)));
	double A = pz/cos(theta2);
	double C = A - a4;
	double D = (a3*a3 - a5*a5 - B*B - C*C)/((-2)*a5*sqrt(B*B+C*C));
	theta5 = acos(D) + atan2(B,C);//theta5 = asin(D) - atan2(C,B);
	theta3 = atan2((B-a5*sin(theta5)),(C-a5*cos(theta5)));
	theta6 = -theta2;

	foot_jointAng[0] = theta1;
	foot_jointAng[1] = -theta2;
	foot_jointAng[2] = -theta3;
	foot_jointAng[3] = theta5;
	foot_jointAng[4] = theta6;

	if (abs(theta1)<M_PI/2 && abs(theta2)<M_PI/2 &&
		abs(theta3)<M_PI/2 &&	abs(theta5)<M_PI/2 &&
		abs(theta6)<M_PI/2 )//馬達限制條件
	{
		for(int i=0; i<5; i++)
		{
			int index = motor_index[i];
			foot_jointAng[i] = (foot_jointAng[i] * Rad2Deg) - this->Robotis[index]->AngleBias;
			joint_position[i] = (int)(foot_jointAng[i] * 3 / this->Robotis[index]->GetResolution());
			joint_position[i] = (this->Robotis[index]->GetPositionMax()/2) + this->Robotis[index]->RotatePolarity*joint_position[i];
			if(joint_position[i] < this->Robotis[index]->GetPositionMin() || joint_position[i] > this->Robotis[index]->GetPositionMax())
			{
        return false;
      }
    }
		return true;
	}
	else
	{
		return false;
	}
}
//---------------------------------------------------------------------------

void Robot::Kinematics(int motor_index[5], int joint_position[5], float *position)
{
	float theta1;
	float theta2;
	float theta3;
	float theta5;
	float theta6;

	int dp;
	float dtheta;
	float foot_jointAng[5];

	for(int i=0; i<5; i++)
	{
		int index = motor_index[i];
		dp = (joint_position[i] - (this->Robotis[index]->GetPositionMax()/2)) /
					this->Robotis[index]->RotatePolarity;
		dtheta = float(dp) / 3 * this->Robotis[index]->GetResolution();
		foot_jointAng[i] = (dtheta + this->Robotis[index]->AngleBias) * Deg2Rad;
	}

	theta1 = foot_jointAng[0];
	theta2 = -foot_jointAng[1];
	theta3 = -foot_jointAng[2];
	theta5 = foot_jointAng[3];
	theta6 = foot_jointAng[4];

	position[0] = cos(theta1) * sin(theta2) * (KNEE_Z + THIGH_Z*cos(theta3) + THIGH_Z*cos(theta5)) +
								sin(theta1) * (THIGH_Z*sin(theta3) + THIGH_Z*sin(theta5));
	position[1] = sin(theta1) * sin(theta2) * (KNEE_Z + THIGH_Z*cos(theta3) + THIGH_Z*cos(theta5)) -
								cos(theta1) * (THIGH_Z*sin(theta3) + THIGH_Z*sin(theta5));
	position[2] = cos(theta2) * (KNEE_Z + THIGH_Z*cos(theta3) + THIGH_Z*cos(theta5));
}

//void Robot_body::Rfoot_invKinematic(float px,float py,float pz,float HipYawAng,float *foot_jointAng)
//{
//	if (foot_invKinematic(px,py,pz,HipYawAng,foot_jointAng))//馬達限制條件
//	{
//		float act_motorAng[5];
//		act_motorAng[0] = dJointGetUniversalAngle2(waist.R_hip);
//		act_motorAng[1] = dJointGetUniversalAngle1(waist.R_hip);
//		act_motorAng[2] = dJointGetHingeAngle(waist.R_Leg.UpRFjoint);
//		act_motorAng[3] = dJointGetHingeAngle(waist.R_Leg.LoRFjoint);
//		act_motorAng[4] = dJointGetHingeAngle(waist.R_Leg.ankle.AnkleJoint);
//
//		float pidOutput[5];
//		for (int i=0 ; i<5 ; i++)
//		{
//			pidOutput[i] = PIDcal(foot_jointAng[i],act_motorAng[i]);
//		}
//
//		// 右腳
//		setMotorRLEG_1(foot_jointAng[0],abs(pidOutput[0]));
//		setMotorRLEG_2(foot_jointAng[1],abs(pidOutput[1]));
//		setMotorRLEG_3(foot_jointAng[2],abs(pidOutput[2]));
//		setMotorRLEG_4(foot_jointAng[3],abs(pidOutput[3]));
//		setMotorRLEG_5(foot_jointAng[4],abs(pidOutput[4]));
//	}
//}
//void Robot_body::Lfoot_invKinematic(float px,float py,float pz,float HipYawAng,float *foot_jointAng)
//{
//	if (foot_invKinematic(px,py,pz,HipYawAng,foot_jointAng))//馬達限制條件
//	{
//		float act_motorAng[5];
//		act_motorAng[0] = dJointGetUniversalAngle2(waist.L_hip);
//		act_motorAng[1] = dJointGetUniversalAngle1(waist.L_hip);
//		act_motorAng[2] = dJointGetHingeAngle(waist.L_Leg.UpRFjoint);
//		act_motorAng[3] = dJointGetHingeAngle(waist.L_Leg.LoRFjoint);
//		act_motorAng[4] = dJointGetHingeAngle(waist.L_Leg.ankle.AnkleJoint);
//
//		float pidOutput[5];
//		for (int i=0 ; i<5 ; i++)
//		{
//			pidOutput[i] = PIDcal(foot_jointAng[i],act_motorAng[i]);
//		}
//
//		// 左腳
//		setMotorLLEG_1(foot_jointAng[0],abs(pidOutput[0]));
//		setMotorLLEG_2(foot_jointAng[1],abs(pidOutput[1]));
//		setMotorLLEG_3(foot_jointAng[2],abs(pidOutput[2]));
//		setMotorLLEG_4(foot_jointAng[3],abs(pidOutput[3]));
//		setMotorLLEG_5(foot_jointAng[4],abs(pidOutput[4]));
//	}
//}
